/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.model;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.camera.TargetOrientation;
import com.team1160.breakaway.util.Point2D;
import com.team1160.breakaway.util.Point2DDouble;

/**
 * This class is used as a parameter for
 * the TaskManager class.
 *
 * @author Team 1160 Firebird Robotics (Saketh Kasibatla)
 *
 */
public class RobotState
{
    //protected instance fields
    ////////////////////////////////////////////////////////////////////////////
    /**
     * A boolean indicating that the dribbler is running
     */
    protected boolean dr_isRunning;

    /**
     * A boolean indicating that the kicker is kicking
     */
    protected boolean k_isKicking;

    protected boolean k_latching;

    /**
     * These values indicate the speeds of each drivetrain motor.
     */
    protected double m_dt_frontLeft, m_dt_frontRight, m_dt_backLeft, m_dt_backRight;

    /**
     * This double indicates the angle of the gyrometer.
     */
    protected double g_angle;

    /**
     * This double indicates the acceleration rate.
     */
    protected double a_rate;

    /**
     * orientation of the robot
     */
    protected int targetOrientation;
    protected Point2DDouble normalizedTarget;
    protected Point2D target;
    protected TargettingState targettingInfo;

    //---------------------------------------------------------------------

    /**
     * This value is 0 when the robot is too far to the left,
     * 2 if the robot is too far to the right, and 1 if the
     * robot is on target, and -1 if the robot can't see a target.
     */
    //protected byte robotOnTarget;

    //constructors
    ////////////////////////////////////////////////////////////////////////////
    /**
     * The constructor.
     * Sets values to 0.0
     */
    public RobotState()
    {
        this.dr_isRunning=false;
        this.k_isKicking=false;
        this.m_dt_backLeft=0.0;
        this.m_dt_backRight=0.0;
        this.m_dt_frontLeft=0.0;
        this.m_dt_frontRight=0.0;
        this.g_angle=0.0;
        this.a_rate=0.0;
        this.targetOrientation = TargetOrientation.NOT_FOUND;
        this.target = null;
        this.normalizedTarget = null;
        this.targettingInfo = new TargettingState();
    }

    //---------------------------------------------------------------------

    //accessor methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * This tells you wether or not the dribbler is running
     * @return the boolean dr_isRunning stored in this class.
     */
    public boolean isDribblerRunning(){
        return this.dr_isRunning;
    }

    //---------------------------------------------------------------------

    /**
     * This tells you wether or not the kicker is kicking
     * @return the boolean k_isKicking stored in this class.
     */
    public boolean isKickerKicking(){
        return this.k_isKicking;
    }

    //---------------------------------------------------------------------

    public boolean latched(){
        return this.k_latching;
    }

    //---------------------------------------------------------------------

    /**
     * Gets the value of a drivetrain motor given the ID
     * @param motorID the ID of the motor values in the constants class
     * @return the value of hte motor
     * @throws IllegalArgumentException thrown if the value is not one fo the constants.
     * @see Constants
     */
    public double getDriveTrainMotorValue(byte motorID)throws IllegalArgumentException{
        switch(motorID){
            case Constants.ID_DRIVETRAIN_LEFT_FRONT:
                return this.m_dt_frontLeft;
            case Constants.ID_DRIVETRAIN_RIGHT_FRONT:
                return this.m_dt_frontRight;
            case Constants.ID_DRIVETRAIN_LEFT_BACK:
                return this.m_dt_backLeft;
            case Constants.ID_DRIVETRAIN_RIGHT_BACK:
                return this.m_dt_backRight;
            default:
                throw new IllegalArgumentException("ERROR: Illegal motor ID, use the values in the Constants class.");
        }
    }

    //---------------------------------------------------------------------

    /**
     * Gets the angle of the gyrometer
     * @return the angle
     */
    public double getGyroAngle(){
        return this.g_angle;
    }

    //---------------------------------------------------------------------


    public void setLatched(boolean latched){
        this.k_latching = latched;
    }

    //---------------------------------------------------------------------

    /**
     * Gets the rate of the accelerometer
     * @return the rate
     */
    public double getAccelRate(){
        return this.a_rate;
    }

    //---------------------------------------------------------------------

    public void setNormalizedTarget(Point2DDouble normTarget)
    {
        this.normalizedTarget = normTarget;
        updateTargetOrientation();
    }

    //---------------------------------------------------------------------

    public Point2DDouble getNormalizedTarget()
    {
        return this.normalizedTarget;
    }

    //---------------------------------------------------------------------

    public void setTarget(Point2D target)
    {
        this.target = target;
    }

    //---------------------------------------------------------------------

    public TargettingState getTargettingInfo()
    {
        return this.targettingInfo;
    }

    //---------------------------------------------------------------------

    protected void updateTargetOrientation()
    {
        if (this.normalizedTarget == null)
        {
            this.targetOrientation = TargetOrientation.NOT_FOUND;
            return;
        }
        double x = this.normalizedTarget.getX();
        if (x < (-1 * Constants.CENTER_HALF_RANGE_NORMALIZED))
        {
            this.targetOrientation = TargetOrientation.LEFT;
        }
        else if (x > Constants.CENTER_HALF_RANGE_NORMALIZED)
        {
            this.targetOrientation = TargetOrientation.RIGHT;
        }
        else
        {
            this.targetOrientation = TargetOrientation.CENTER;
        }
    }
    
    //---------------------------------------------------------------------

    
    public Point2D getTarget()
    {
        return this.target;
    }

    //---------------------------------------------------------------------


    public int getTargetOrientation()
    {
        return this.targetOrientation;
    }

    //---------------------------------------------------------------------

//    /**
//     * This method gets the robotOnTarget value in this class
//     * @return the value of robotOnTarget
//     */
//    public byte getRobotOnTarget(){
//        return this.robotOnTarget;
//    }

    //mutator methods
    ////////////////////////////////////////////////////////////////////////////

    //---------------------------------------------------------------------


    /**
     * This method sets the boolean dr_isRunning
     * @param value the value to set dr_isRunning to.
     */
    public void setDribblerRunning(boolean value){
        this.dr_isRunning=value;
    }

    //---------------------------------------------------------------------

    /**
     * This method set the boolean k_isKicking.
     * @param value the value to set k_isKicking to.
     */
    public void setKickerKicking(boolean value){
        this.k_isKicking=value;
    }

    //---------------------------------------------------------------------

    /**
     * Sets the motor value with the given ID to the given value
     * @param motorID the ID. use the Constants class
     * @param value the value to set
     * @throws IllegalArgumentException throws if the ID not in  the constants class
     * @see Constants
     */
    public void setDriveTrainMotorValue(byte motorID, double value)throws IllegalArgumentException{
        switch(motorID){
            case Constants.ID_DRIVETRAIN_LEFT_FRONT:
                this.m_dt_frontLeft=value;
                break;
            case Constants.ID_DRIVETRAIN_RIGHT_FRONT:
                this.m_dt_frontRight=value;
                break;
            case Constants.ID_DRIVETRAIN_LEFT_BACK:
                this.m_dt_backLeft=value;
                break;
            case Constants.ID_DRIVETRAIN_RIGHT_BACK:
                this.m_dt_backRight=value;
                break;
            default:
                throw new IllegalArgumentException("ERROR: Illegal motor ID, use the values in the Constants class.");
        }
    }

    //---------------------------------------------------------------------


    /**
     * Sets the gyro angle variable to the given value
     * @param value teh given value
     */
    public void setGyroAngle(double value){
        this.g_angle=value;
    }

    //---------------------------------------------------------------------

    /**
     * Sets the acceleration rate to the given value
     * @param value given value
     */
    public void setAccelRate(double value){
        this.a_rate = value;
    }

    //---------------------------------------------------------------------


//    public void setTargetOrientation(int orientationId)
//    {
//        if (orientationId != TargetOrientation.NOT_FOUND &&
//            orientationId != TargetOrientation.LEFT &&
//            orientationId != TargetOrientation.CENTER &&
//            orientationId != TargetOrientation.RIGHT)
//        {
//            return;
//        }
//        else
//            this.targetOrientation = orientationId;
//    }

//    /**
//     * sets the robotOnTarget variable to the giver nalue
//     * @param value the given value
//     */
//    public void setRobotOnTarget(byte value){
//        this.robotOnTarget = value;
//    }

    public String toString(){
        String retr="";
        retr += "Dribbling: ";
        retr += new Boolean(this.dr_isRunning).toString();
        retr += "\n";
        
        retr += "Kicking: ";
        retr += new Boolean(this.k_isKicking).toString();
        retr += "\n\n";
        
        retr += "Drivetrain Motors\n";
        
        retr += "Front Left: ";
        retr += Double.toString(this.m_dt_frontLeft);
        retr += "\n";
        
        retr += "Front Right: ";
        retr += Double.toString(this.m_dt_frontRight);
        retr += "\n";
        
        retr += "Back Left: ";
        retr += Double.toString(this.m_dt_backLeft);
        retr += "\n";
        
        retr += "Back Right: ";
        retr += Double.toString(this.m_dt_backRight);
        retr += "\n\n";
        
        retr += "Gyrometer Angle: ";
        retr += Double.toString(this.g_angle);
        retr += "\n";

        retr += "Accelerometer Rate: ";
        retr += Double.toString(this.a_rate);
        retr += "\n";

        return retr;
    }

    //---------------------------------------------------------------------


}
